#include "pathfinder.h"

#include "gamestate.h"

#include "entities/tile.h"

#include "primitives/graph.h"

Pathfinder pathfinder;

std::vector<int> Pathfinder::GetPath(int start, int finish, bool overrideOccupiedFinish){
    std::cout<<"Getting path from "<<start<<" to "<<finish<<std::endl;
    int lowest;
    open.clear();
    closed.clear();
    path.clear();
//    if(overrideOccupiedFinish){
//        gamestate->GetTileByIndex(finish)->SetOccupied(false);
//    }

    if(start>gamestate->MapSize() or finish>gamestate->MapSize()){
        return path;
    }

    for(unsigned int i=0;i<gamestate->MapSize();i++){
        gamestate->GetTileByIndex(i)->ResetPathfinding();
    }

	open.push_back(start);
	while(true){
		if(open.size() == 0){
			break;
		}
		lowest = FindLowestF();
		if(open.at(lowest) == finish){
			break;
		}
		// Add the node with lowest F cost and move it to the closed list
		closed.push_back(open.at(lowest));
		open.erase(open.begin()+lowest);

		// Swap index in open lowest for node index lowest
		lowest = closed.back();
		// Neighbours
		int bl = lowest-gamestate->MapWidth()-1;
		int bc = lowest-gamestate->MapWidth();
		int br = lowest-gamestate->MapWidth()+1;
		int l  = lowest-1;
		int r  = lowest+1;
		int ul = lowest+gamestate->MapWidth()-1;
		int uc = lowest+gamestate->MapWidth();
		int ur = lowest+gamestate->MapWidth()+1;

		// Check that each neighbouring node is within the boundaries and is not in the open or closed lists
		// Add the index to the open list, set ParentID, parent direction and calculate F,G and H
		if(lowest > gamestate->MapWidth() and lowest%gamestate->MapWidth() != 0)
			if(IsInOpenList(bl) == false and IsInClosedList(bl) == false){// and !gamestate->GetTileByIndex(bl)->IsOccupied()){
				open.push_back(bl);
				gamestate->GetTileByIndex(bl)->SetParentDirection(BELOWLEFT);
				gamestate->GetTileByIndex(bl)->ParentID = lowest;
				CalculateFGH(lowest, bl, finish);
			}
		if(lowest > gamestate->MapWidth())
			if(IsInOpenList(bc) == false and IsInClosedList(bc) == false){// and !gamestate->GetTileByIndex(bc)->IsOccupied()){
				open.push_back(bc);
				gamestate->GetTileByIndex(bc)->SetParentDirection(BELOW);
				gamestate->GetTileByIndex(bc)->ParentID = lowest;
				CalculateFGH(lowest, bc, finish);
			}
		if(lowest > gamestate->MapWidth() and lowest%gamestate->MapWidth() != gamestate->MapWidth()-1)
			if(IsInOpenList(br) == false and IsInClosedList(br) == false){// and !gamestate->GetTileByIndex(br)->IsOccupied()){
				open.push_back(br);
				gamestate->GetTileByIndex(br)->SetParentDirection(BELOWRIGHT);
				gamestate->GetTileByIndex(br)->ParentID = lowest;
				CalculateFGH(lowest, br, finish);
			}
		if(lowest%gamestate->MapWidth() != 0)
			if(IsInOpenList(l) == false and IsInClosedList(l) == false){// and !gamestate->GetTileByIndex(l)->IsOccupied()){
				open.push_back(l);
				gamestate->GetTileByIndex(l)->SetParentDirection(LEFT);
				gamestate->GetTileByIndex(l)->ParentID = lowest;
				CalculateFGH(lowest, l, finish);
			}
		if(lowest%gamestate->MapWidth() != gamestate->MapWidth()-1)
			if(IsInOpenList(r) == false and IsInClosedList(r) == false){// and !gamestate->GetTileByIndex(r)->IsOccupied()){
				open.push_back(r);
				gamestate->GetTileByIndex(r)->SetParentDirection(RIGHT);
				gamestate->GetTileByIndex(r)->ParentID = lowest;
				CalculateFGH(lowest, r, finish);
			}
		if(lowest < gamestate->MapSize()-gamestate->MapWidth() and lowest%gamestate->MapWidth() != 0)
			if(IsInOpenList(ul) == false and IsInClosedList(ul) == false){// and !gamestate->GetTileByIndex(ul)->IsOccupied()){
				open.push_back(ul);
				gamestate->GetTileByIndex(ul)->SetParentDirection(ABOVELEFT);
				gamestate->GetTileByIndex(ul)->ParentID = lowest;
				CalculateFGH(lowest, ul, finish);
			}
		if(lowest < gamestate->MapSize()-gamestate->MapWidth()){
			if(IsInOpenList(uc) == false and IsInClosedList(uc) == false){// and !gamestate->GetTileByIndex(uc)->IsOccupied()){
				open.push_back(uc);
				gamestate->GetTileByIndex(uc)->SetParentDirection(ABOVE);
				gamestate->GetTileByIndex(uc)->ParentID = lowest;
				CalculateFGH(lowest, uc, finish);
			}
        }
		if(lowest < gamestate->MapSize()-gamestate->MapWidth() and lowest%gamestate->MapWidth() != gamestate->MapWidth()-1){
			if(IsInOpenList(ur) == false and IsInClosedList(ur) == false){// and !gamestate->GetTileByIndex(ur)->IsOccupied()){
                open.push_back(ur);
                gamestate->GetTileByIndex(ur)->SetParentDirection(ABOVERIGHT);
                gamestate->GetTileByIndex(ur)->ParentID = lowest;
                CalculateFGH(lowest, ur, finish);
			}
        }
	}
	int id = finish;
	while(true){
	    if(id==-1){
	        std::cout<<"Error: Invalid tile in path."<<std::endl;
	        break;
	    }
		path.push_back(id);
		id = gamestate->GetTileByIndex(id)->ParentID;
		if(id == start){
			path.push_back(id);
			break;
		}
	}
//	if(overrideOccupiedFinish){
//        gamestate->GetTileByIndex(finish)->SetOccupied(true);
//	}
	return path;
}

void Pathfinder::CalculateFGH(int current, int next, int finish){
    Tile* tile = gamestate->GetTileByIndex(next);
    Tile* parent = gamestate->GetTileByIndex(tile->ParentID);
    Graph roads = gamestate->GetRoads();

    float road_multiplier = roads.Adjacent(current, next) ? roads.Weight(current, next) : 1;

	if(tile->DiagonalParent()){
		tile->G = parent->G + 10*1.4/road_multiplier;
	}
	else{
		tile->G = parent->G + 10/road_multiplier;
	}

	int c = gamestate->Distance(finish, next);
	tile->H = (c/gamestate->MapWidth()+c%gamestate->MapWidth())*10;
	if(tile->H < 0)
		tile->H*=-1;
	if(tile->G < 0)
		tile->G*=-1;

	tile->F = tile->G + tile->H;
}

bool Pathfinder::IsInOpenList(int id){
	for(unsigned int i=0;i<open.size();i++){
		if(open.at(i) == id)
			return true;
	}
	return false;
}

bool Pathfinder::IsInClosedList(int id){
	for(unsigned int i=0;i<closed.size();i++){
		if(closed.at(i) == id)
			return true;
	}
	return false;
}

int Pathfinder::FindLowestF(){
	int id = 0;
	// Get first index from open nodes
//	std::cout<<open.at(0)<<std::endl;
	int F = gamestate->GetTileByIndex(open.at(0))->F;

	for(unsigned int i=0; i<open.size(); i++){
		if(gamestate->GetTileByIndex(open.at(i))->F < F){
			id = i;
			F = gamestate->GetTileByIndex(open.at(i))->F;
		}
	}
	return id;
}
